//****************************************************************************
// auto_task.c -
//****************************************************************************

#include "inc/hw_types.h"
#include "utils/scheduler.h"
#include "drivers/motor.h"
#include "drivers/sensors.h"
#include "drivers/io.h"

#include "helper.h"
#include "auto_task.h"
#include "drive_task.h"
#include "lineIR_task.h"
#include "wireless_task.h"


//****************************************************************************
//
// Define the possible states for the EvalBot autonomous state machine
//
//****************************************************************************
#define SHORT_RUN    1  // Ticks
#define CORNER_SHORT 2  // Ticks

#define FULL_SPEED   80
#define MED_SPEED    80
#define TURN_SPEED   70


//****************************************************************************
//
// Global variables
//
//****************************************************************************
static unsigned long g_ulTargetLeftWheelTicks;
static unsigned long g_ulTargetRightWheelTicks;
static tBoolean g_bRRun = false;
static tBoolean g_bLRun = false;

//****************************************************************************
//
// Prototypes
//
//****************************************************************************
tBoolean TickChecker(void);
void MotorWireless(unsigned char ucLeftTicks, unsigned char ucRightTicks, unsigned char ucLSpeed,
	      unsigned char ucRSpeed, unsigned char command);

//****************************************************************************
//
// This "task" is called periodically from the scheduler in the main app.
// It runs a state machine that tracks the EvalBot motion, and changes
// the motion in reaction to external inputs.
//
//****************************************************************************
void AutoTask(void *pvParam)
{

  /* Check if we are finished processing the wheel ticks 
   * Execute another action if we are done
   */
  if (TickChecker() == true) {

    switch(getLineIRState()) {

    case 0:
      {
	// No Line visible
	MotorWireless(SHORT_RUN, SHORT_RUN, FULL_SPEED, FULL_SPEED, 
		      MOTOR_DRIVE_FORWARD);
	break;
      }
    case 1:
      {
	// Left side hit
	MotorWireless(SHORT_RUN, SHORT_RUN, TURN_SPEED, TURN_SPEED, 
		      MOTOR_DRIVE_TURN_LEFT);
	break;
      }
    case 2:
      {
	// Center hit
	MotorWireless(SHORT_RUN, SHORT_RUN, FULL_SPEED, FULL_SPEED, 
		      MOTOR_DRIVE_FORWARD);
	break;
      }
    case 3:
      {
	// Looks like corner to the left
	MotorWireless(SHORT_RUN, CORNER_SHORT, TURN_SPEED, TURN_SPEED, 
		      MOTOR_DRIVE_TURN_LEFT);
	break;
      }
    case 4:
      {
	// Right side hit
	MotorWireless(SHORT_RUN, SHORT_RUN, TURN_SPEED, TURN_SPEED, 
		      MOTOR_DRIVE_TURN_RIGHT);
	break;
      }
    case 5:
      {
	// Appears to be two lines... move a tick and re-assess
	MotorWireless(SHORT_RUN, SHORT_RUN, MED_SPEED, MED_SPEED, 
		      MOTOR_DRIVE_FORWARD);
	break;
      }
    case 6:
      {
	// Looks like a corner to the right
	MotorWireless(CORNER_SHORT, SHORT_RUN, TURN_SPEED, TURN_SPEED, 
		      MOTOR_DRIVE_TURN_RIGHT);
	break;
      }
    case 7:
      {
	// We seem to be perpendicular, turn and re-assess
	MotorWireless(SHORT_RUN, 0, MED_SPEED, TURN_SPEED, 
		      MOTOR_DRIVE_TURN_RIGHT);
	break;
      }
    default:
      {
	// Unknown state, just stop
	DriveRun(MOTOR_DRIVE_FORWARD, 0, 0);
	DriveStop();
	break; 
      }
    }
  }

} /* AutoTask */

//****************************************************************************

//****************************************************************************
//
// Misc Functions
//
//****************************************************************************

/* Check if there are ticks to process
 * Return true if we are done processing
 */
tBoolean
TickChecker(void) {
  /* Run/stop left motor based on target ticks */
  if (getTickLeft() < g_ulTargetLeftWheelTicks) {
    // Let the left motor keep running
    g_bLRun = true;
  }
  else {
    DriveStopOne(MOTOR_DRIVE_LEFT);
    g_bLRun = false;
  }
  /* Run/stop right motor based on target ticks */
  if (getTickRight() < g_ulTargetRightWheelTicks) {
    // Let the right motor keep running
    g_bRRun = true;
  }
  else {
    DriveStopOne(MOTOR_DRIVE_RIGHT);
    g_bRRun = false;
  }

  if ((g_bLRun == false) && (g_bRRun == false)) {
    DriveRun(MOTOR_DRIVE_FORWARD, 0, 0);
    return true;
  }
  else {
    return false;
  }

}

/* Function execute motor actions and send them wirelessly
 */
void
MotorWireless(unsigned char ucLeftTicks, unsigned char ucRightTicks, unsigned char ucLSpeed,
	      unsigned char ucRSpeed, unsigned char command) {

  DanceInstruction instruction;
  unsigned char ucRightDirection;
  unsigned char ucLeftDirection;
  static unsigned int numSent = 0;
  numSent += 1;
 if (command == MOTOR_DRIVE_TURN_RIGHT) {
   ucRightDirection = 0;
   ucLeftDirection = 1;
 }
 else if (command == MOTOR_DRIVE_TURN_LEFT) {
   ucRightDirection = 1;
   ucLeftDirection = 0;
 }
 else if (command == MOTOR_DRIVE_FORWARD) {
   ucRightDirection = 1;
   ucLeftDirection = 1;
 }
 else if (command == MOTOR_DRIVE_REVERSE) {
   ucRightDirection = 0;
   ucLeftDirection = 0;
 }

  g_ulTargetLeftWheelTicks = getTickLeft() + ucLeftTicks;
  g_ulTargetRightWheelTicks = getTickRight() + ucRightTicks;
  DriveRun(command, ucLSpeed, ucRSpeed);

  instruction.rightWheelSpeed = ucRSpeed;
  instruction.leftWheelSpeed = ucLSpeed;
  instruction.rightTickCount = ucRightTicks;
  instruction.leftTickCount = ucLeftTicks;
  instruction.rightDirection = ucRightDirection;
  instruction.leftDirection = ucLeftDirection;
  displayMessageAndNumber("Messages", numSent);
  SendInstruction(instruction);

}

// This function is used to perform any needed initialization for the
// autonomous driving "task".
void
AutoTaskInit(void)
{
  g_ulTargetLeftWheelTicks = 0;
  g_ulTargetRightWheelTicks = 0;
  DriveRun(MOTOR_DRIVE_FORWARD, 0, 0);
  DriveStop();
  //PushButtonsInit();
  //BumpSensorsInit();
}
